`timescale 1 ns / 100 ps

`include "sd_defines.vh"

`define BIN_DIR "../bin"
`define LOG_DIR "../log"

`define SD_BASE 32'hd0000000
`define CMD2 16'h200
`define CMD3 16'h300
`define ACMD6 16'h600
`define CMD7 16'h700
`define CMD8 16'h800
`define CMD55 16'h3700
`define ACMD41 16'h2900
`define CMD8 16'h800
`define CMD9 16'h900
`define CMD12 16'h0C00
`define CMD16 16'h1000
`define CMD17 16'h1100
`define CMD18 16'h1200
`define CMD24 16'h1800

`define RSP_136 16'h2
`define RSP_48 16'h1
`define CICE 16'h10
`define CRCE 16'h08
`define CRD 16'h20
`define CWD 16'h40

module sdc_ahb_wrap_tb;

initial begin
    $dumpfile("sdc_ahb_wrap_tb.vcd");        //生成的vcd文件名称
    $dumpvars(0, sdc_ahb_wrap_tb);    //tb模块名称
    $timeformat(-9, 2, "ns", 4);
end

localparam TCLK = 20;

reg clk;
reg rst_n;

initial begin
    #0 clk = 0;
    forever
        #(TCLK / 2) clk = ~clk;
end

initial begin
    #0 rst_n = 0;
    #20 rst_n = 1;
end

reg [7:0]   HADDR;
reg [31:0]  HWDATA;
reg         HWRITE;
reg [2:0]   HSIZE;
reg [2:0]   HBURST;
reg         HSEL;
reg [1:0]   HTRANS;
reg         HREADY_IN;

wire         HREADY_OUT;
wire [31:0]  HRDATA;
wire [1:0]   HRESP;

wire        mHSEL;
wire [2:0]  mHSIZE;
wire [31:0] mHRDATA;
wire [1:0]  mHRESP;
wire        mHREADY;
wire        mHREADYOUT;
wire        mHWRITE;
wire [2:0]  mHBURST;
wire [31:0] mHADDR;
wire [1:0]  mHTRANS;
wire [31:0] mHWDATA;

reg print_mem;

initial begin
    HADDR = 32'b0;
    HWRITE = 1'b0;
    HWDATA = 32'b0;

    HSIZE = 3'b0;
    HBURST = 3'b0;
    HSEL = 1'b0;
    HTRANS = 2'b00;
    HREADY_IN = 1'b1;
end

task ahb_read;
    input [7:0] addr;
    output integer value;
    begin
        @(posedge clk);
        #1;

        HSEL = 1'b1;
        HTRANS = 2'b10;
        HADDR = addr;
        HWRITE = 1'b0;
        HWDATA = {32{1'bx}};
        HSIZE = 3'b010;

        @(posedge clk);
        #1;
        HTRANS = 2'b00;

        repeat(4) begin
            @(posedge clk);
            if(HREADY_OUT) begin
                #1;
                // $display("[%m]#%t INFO: Read Value: 0x%08x @ 0x%08x", $time, HRDATA, HADDR);
                value = HRDATA;
                HSEL = 1'b0;
                HADDR = 8'h0;
                HWRITE = 1'b0;
                HWDATA = {32{1'bx}};
                HSIZE = 3'b000;

                HTRANS = 2'b0;
                disable ahb_read;
            end
        end
        $stop;
    end
endtask

task ahb_write;
    input [7:0] addr;
    input integer data;
    begin
        @(posedge clk);
        #1;

        HSEL = 1'b1;
        HTRANS = 2'b10;
        HADDR = addr;
        HWRITE = 1'b1;
        HWDATA = {32{1'bx}};
        HSIZE = 3'b010;

        @(posedge clk);
        #1;

        HTRANS = 2'b00;
        HWDATA = data;

        repeat(4) begin
            @(posedge clk);
            if(HREADY_OUT) begin
                #1;
                // $display("[%m]#%t INFO: Write Value: 0x%08x @ 0x%08x", $time, HWDATA, HADDR);
                HSEL = 1'b0;
                HADDR = 8'h0;
                HWRITE = 1'b0;
                HWDATA = {32{1'bx}};
                HSIZE = 3'b000;

                HTRANS = 2'b0;
                disable ahb_write;
            end
        end
        $stop;
    end
endtask

task print_memory;
    begin
        $display("%c[1;35m [%m]#%t INFO: Print local-memory %c[1;0m", 27, $time, 27);
        print_mem = 1;
        @(posedge clk);
        #(TCLK / 2);
        print_mem = 0;
    end
endtask

function [7:0] reg_addr;
    input [7:0] offset;
    begin
        reg_addr = `SD_BASE + offset;
    end
endfunction

task get_resp_reg;
    input integer reg_num;
    output integer reg_val;
    begin
        if(reg_num >= 0 && reg_num < 4) begin
            ahb_read(reg_addr(`resp0 + 4*reg_num), reg_val);
        end
        else begin
            $display("ERROR: Invaild parameter");
        end
    end
endtask

task send_cmd;
    input integer command;
    input integer argument;
    output integer status;
    integer retry;
    begin
        ahb_write(reg_addr(`command), command);
        ahb_write(reg_addr(`argument), argument);
        ahb_read(reg_addr(`cmd_isr), status);
        while (status[`INT_CMD_CC] != 1 && status[`INT_CMD_EI] != 1) begin
            ahb_read(reg_addr(`cmd_isr), status);
        end

        if(retry == 0) begin
            $display("Retry: Send command timeout");
            $stop;
        end

        ahb_write(reg_addr(`cmd_isr), 0);
    end
endtask

task setup_core;
    input integer cmd_timeout;
    input integer data_timeout;
    begin
        ahb_write(reg_addr(`reset), 1);
        ahb_write(reg_addr(`cmd_timeout), cmd_timeout);
        ahb_write(reg_addr(`data_timeout), data_timeout);
        ahb_write(reg_addr(`clock_d), 16'h0); // clock devider
        ahb_write(reg_addr(`reset), 0);
        ahb_write(reg_addr(`cmd_iser), 5'h1f);
        ahb_write(reg_addr(`data_iser), 3'h7);
        ahb_write(reg_addr(`controller), 1); // 4bit bus
        $display("[%m]#%t INFO: Setup SDCard Controller ... OK", $time);
    end
endtask

task setup_dma;
    input integer address;
    begin
        ahb_write(reg_addr(`dst_src_addr), address);
    end
endtask

task send_op_cond;
    output integer resp_data;
    begin
        resp_data[31]=1;
        while (resp_data[31]) begin
            send_cmd(`CMD55 |`CICE | `CRCE | `RSP_48, 0, status); //Send CMD 55
            send_cmd(`ACMD41 | `RSP_48, 0, status); //Send ACMD 41
            get_resp_reg(0, resp_data);
        end
    end
endtask

task setup_card_to_transfer;
    input integer rca;
    output integer status;
    // integer resp_data;
    begin
        $display("[%m]#%t INFO: [CMD7] Select SDCard", $time);
        send_cmd(`CMD7 |  `CRCE | `CRCE | `RSP_48, rca << 16, status);
        send_cmd(`CMD55 |`CICE | `CRCE | `RSP_48, 0, status);
        $display("[%m]#%t INFO: [ACMD6] Set Bus-Width = 4", $time);
        send_cmd(`ACMD6 |`CICE | `CRCE | `RSP_48, 2, status);
        // get_resp_reg(0, resp_data);
        if(status) begin
            $display("\tSet Bus-Width = 4 ... OK");
        end else begin
            $display("\tSet Bus-Width = 4 ... ERR");
        end
    end
endtask

task init_card;
    output integer status;
    output integer rca;
    output integer high_cap;
    integer resp_data;
    integer retry;
    integer cid[3:0];
    begin
        $display("[%m]#%t INFO: Init SDCard", $time);

        setup_core(16'hffff, 16'hffff);

        $display("[%m]#%t INFO: [CMD0] Reset SDCard", $time);
        send_cmd(0, 0, status); // CMD 0: Reset

        $display("[%m]#%t INFO: [CMD8] Check Voltage", $time);
        send_cmd(`CMD8 | `RSP_48, 0, status); // CMD 8: Get Card Type
        if (status[`INT_CMD_EI] == 1)
            $display("\t[Detect] => V2.0 Card");

        $display("[%m]#%t INFO: [ACMD41] Send OP-Cond", $time);
        send_op_cond(resp_data);

        if(resp_data & 32'h40000000 == 32'h40000000) begin
            $display("[%m]#%t INFO: Support High-Capacity", $time);
            high_cap = 1;
        end else begin
            $display("[%m]#%t WARN: Not support High-Capacity", $time);
            high_cap = 0;
        end

        $display("[%m]#%t INFO: [CMD2] Get CID", $time);
        send_cmd(`CMD2 | `CRCE | `RSP_136, 0, status);
        get_resp_reg(0, cid[0]);
        get_resp_reg(1, cid[1]);
        get_resp_reg(2, cid[2]);
        get_resp_reg(3, cid[3]);
        $display("\t[CID] => [%08x-%08x-%08x-%08x]", cid[0], cid[1], cid[2], cid[3]);

        $display("[%m]#%t INFO: [CMD3] Get RCA", $time);
        send_cmd(`CMD3 | `CRCE | `RSP_48, 0, status);
        get_resp_reg(0, resp_data);
        rca = resp_data[31:16];
        $display("\t[RCA] => %08x", rca);

        $display("[%m]#%t INFO: Init SDCard Finished", $time);
    end
endtask

task get_capacity;
    input integer rca;
    input integer high_capacity;
    output integer block_cnt;
    integer resp_data;
    integer status;
    integer csize;
    integer cmult;
    integer csd[3:0];
    begin
        send_op_cond(resp_data);
        $display("[%m]#%t INFO: [CMD9] Get CSD", $time);
        send_cmd(`CMD9 | `CRCE | `RSP_136, rca << 16, status);
        get_resp_reg(0, csd[0]);
        get_resp_reg(1, csd[1]);
        get_resp_reg(2, csd[2]);
        get_resp_reg(3, csd[3]);
        $display("\t[CSD] => [%08x-%08x-%08x-%08x]", csd[0], csd[1], csd[2], csd[3]);


        if(high_capacity) begin
            csize = (csd[1] & 32'h3ff) << 16 | (csd[2] & 32'hffff0000) >> 16;
            cmult = 8;
        end else begin
            csize = (csd[1] & 32'h3ff) << 2 | (csd[2] & 32'hc0000000) >> 30;
            cmult = (csd[2] & 32'h38000) >> 15;
        end

        block_cnt = csize * cmult;
        $display("[%m]#%t INFO: [hight-cap: %1d] Block Count: %1d * %1d = [%4d] (%0d bytes)", $time, high_capacity, csize, cmult, block_cnt, block_cnt * 512);
    end
endtask

task wait_data_isr;
    output integer status;
    begin
        status = 0;
        while (status[`INT_DATA_CC] != 1 && status[`INT_DATA_EI] != 1) begin
            ahb_read(reg_addr(`data_isr), status);
        end
        ahb_write(reg_addr(`data_isr), 0);
    end
endtask

task send_cmd_with_data;
    input integer command;
    input integer argument;
    input integer dma_addr;
    output integer status;
    integer resp_data;
    begin
        setup_dma(dma_addr);
        send_cmd(command, argument, status);
        wait_data_isr(status);
    end
endtask

integer status;
integer rca;
integer resp_data;
integer high_capacity;
integer block_cnt;
initial begin
    @(posedge rst_n);
    repeat(4) @(posedge clk);
    $display("%c[1;34m [%m]#%t INFO: Testbench Start %c[0m", 27, $time, 27);

    init_card(status, rca, high_capacity);

    get_capacity(rca, high_capacity, block_cnt);

    setup_card_to_transfer(rca, status);

    print_memory();

    $display("%c[1;33m [%m]#%t INFO: [CMD17] SDCard Read 512 bytes from [block 0] to 0x%08x %c[0m", 27, $time, 32'h200, 27);
    send_cmd_with_data(`CMD17 | `CRD | `CICE | `CRCE | `RSP_48, 0, 32'h200, status);

    print_memory();

    $display("%c[1;33m [%m]#%t INFO: [CMD24] SDCard Write 512 bytes from 0x%08x to [block 0] %c[0m", 27, $time, 0, 27);
    send_cmd_with_data(`CMD24 | `CWD | `CICE | `CRCE | `RSP_48, 0, 0, status);

    $display("%c[1;33m [%m]#%t INFO: [CMD17] SDCard Read 512 bytes from [block 0] to 0x%08x %c[0m", 27, $time, 32'h200, 27);
    send_cmd_with_data(`CMD17 | `CRD | `CICE | `CRCE | `RSP_48, 0, 32'h200, status);

    print_memory();

    $display("[%m]#%t INFO: [CMD17] Stop transfer", $time);
    send_cmd(`CMD12 |`CICE | `CRCE | `RSP_48, 0, status);

    repeat(128) @(posedge clk);
    $display("%c[1;32m [%m]#%t INFO: Testbench Finished %c[0m", 27, $time, 27);
    $stop;
end


wire sd_clk;
wire sd_cmd;
wire [3:0] sd_dat;

sdc_ahb_wrap dut (
                 .clk(clk),
                 .reset_n(rst_n),

                 .sHADDR(HADDR),
                 .sHWDATA(HWDATA),
                 .sHWRITE(HWRITE),
                 .sHREADYOUT(HREADY_OUT),
                 .sHSIZE(HSIZE),
                 .sHBURST(HBURST),
                 .sHSEL(HSEL),
                 .sHTRANS(HTRANS),
                 .sHRDATA(HRDATA),
                 .sHRESP(HRESP),
                 .sHREADY(HREADY_IN),

                 .mHSEL(mHSEL),
                 .mHSIZE(mHSIZE),
                 .mHRDATA(mHRDATA),
                 .mHRESP(mHRESP),
                 .mHREADY(mHREADY),
                 .mHREADYOUT(mHREADYOUT),
                 .mHWRITE(mHWRITE),
                 .mHBURST(mHBURST),
                 .mHADDR(mHADDR),
                 .mHTRANS(mHTRANS),
                 .mHWDATA(mHWDATA),

                 .sd_clk(sd_clk),
                 .sd_cmd(sd_cmd),
                 .sd_dat(sd_dat)
             );


ahb_mem mem0(
            .clk(clk),
            .reset_n(rst_n),

            .sHSEL(mHSEL),
            .sHSIZE(mHSIZE),
            .sHWRITE(mHWRITE),
            .sHADDR(mHADDR),
            .sHRDATA(mHRDATA),
            .sHWDATA(mHWDATA),
            .sHTRANS(mHTRANS),
            .sHBURST(mHBURST),
            .sHRESP(mHRESP),
            .sHREADYOUT(mHREADY),
            .sHREADY(mHREADYOUT),

            .print(print_mem)
        );

sdModel #(.ramdisk ("./ramdisk2.hex"),
          .log_file ("./sd_model.log")
         ) sdModelTB0(
            .sdClk(sd_clk),
            .cmd(sd_cmd),
            .dat(sd_dat)
        );

endmodule
